#include "EDFrustum.h"

#include <windows.h>

#include "glee.h"
#include <gl/gl.h>
#include <gl/glu.h>


void EDFrustum::Set( const matrix4f *pMatrix, float fRatio, float fNear, float fFar, float fFOV )
{
	m_pMatrix = pMatrix;
	m_fRatio = fRatio;
	m_fNear = fNear;
	m_fFar = fFar;
	m_fFOV = fFOV;

	m_HNear = 2.0f * tan( m_fFOV / 2.0f ) * m_fNear;
	m_WNear = m_HNear * m_fRatio;

	m_HFar = 2.0f * tan( m_fFOV / 2.0f ) * m_fFar;
	m_WFar = m_HFar * m_fRatio;

	vec3f fc;

	fc = pMatrix->axis_pos + pMatrix->axis_z * m_fFar;

	m_points[ftl] = fc + (pMatrix->axis_y * (m_HFar / 2.0f)) - (pMatrix->axis_x * -(m_WFar / 2.0f) );
	m_points[ftr] = fc + (pMatrix->axis_y * (m_HFar / 2.0f)) + (pMatrix->axis_x * -(m_WFar / 2.0f) );
	m_points[fbl] = fc - (pMatrix->axis_y * (m_HFar / 2.0f)) - (pMatrix->axis_x * -(m_WFar / 2.0f) );
	m_points[fbr] = fc - (pMatrix->axis_y * (m_HFar / 2.0f)) + (pMatrix->axis_x * -(m_WFar / 2.0f) );

	vec3f nc;

	nc = pMatrix->axis_pos + pMatrix->axis_z * m_fNear;

	m_points[ntl] = nc + (pMatrix->axis_y * (m_HNear / 2.0f)) - (pMatrix->axis_x * -(m_WNear / 2.0f) );
	m_points[ntr] = nc + (pMatrix->axis_y * (m_HNear / 2.0f)) + (pMatrix->axis_x * -(m_WNear / 2.0f) );
	m_points[nbl] = nc - (pMatrix->axis_y * (m_HNear / 2.0f)) - (pMatrix->axis_x * -(m_WNear / 2.0f) );
	m_points[nbr] = nc - (pMatrix->axis_y * (m_HNear / 2.0f)) + (pMatrix->axis_x * -(m_WNear / 2.0f) );

	GetPlaneEquation( m_Normals[PNEAR], m_Distances[PNEAR], m_points[ntl], m_points[ntr], m_points[nbr] );
	GetPlaneEquation( m_Normals[PFAR], m_Distances[PFAR], m_points[fbl], m_points[fbr], m_points[ftr] );
	GetPlaneEquation( m_Normals[PLEFT], m_Distances[PLEFT], m_points[ftl], m_points[ntl], m_points[nbl] );
	GetPlaneEquation( m_Normals[PRIGHT], m_Distances[PRIGHT], m_points[ntr], m_points[ftr], m_points[fbr] );
	GetPlaneEquation( m_Normals[PTOP], m_Distances[PTOP], m_points[ntl], m_points[ftl], m_points[ftr] );
	GetPlaneEquation( m_Normals[PBOTTOM], m_Distances[PBOTTOM], m_points[fbl], m_points[nbl], m_points[nbr] );
}

void EDFrustum::Update(void)
{
	vec3f fc;

	fc = m_pMatrix->axis_pos + m_pMatrix->axis_z * m_fFar;

	m_points[ftl] = fc + (m_pMatrix->axis_y * (m_HFar / 2.0f)) - (m_pMatrix->axis_x * -(m_WFar / 2.0f) );
	m_points[ftr] = fc + (m_pMatrix->axis_y * (m_HFar / 2.0f)) + (m_pMatrix->axis_x * -(m_WFar / 2.0f) );
	m_points[fbl] = fc - (m_pMatrix->axis_y * (m_HFar / 2.0f)) - (m_pMatrix->axis_x * -(m_WFar / 2.0f) );
	m_points[fbr] = fc - (m_pMatrix->axis_y * (m_HFar / 2.0f)) + (m_pMatrix->axis_x * -(m_WFar / 2.0f) );

	vec3f nc;

	nc = m_pMatrix->axis_pos + m_pMatrix->axis_z * m_fNear;

	m_points[ntl] = nc + (m_pMatrix->axis_y * (m_HNear / 2.0f)) - (m_pMatrix->axis_x * -(m_WNear / 2.0f) );
	m_points[ntr] = nc + (m_pMatrix->axis_y * (m_HNear / 2.0f)) + (m_pMatrix->axis_x * -(m_WNear / 2.0f) );
	m_points[nbl] = nc - (m_pMatrix->axis_y * (m_HNear / 2.0f)) - (m_pMatrix->axis_x * -(m_WNear / 2.0f) );
	m_points[nbr] = nc - (m_pMatrix->axis_y * (m_HNear / 2.0f)) + (m_pMatrix->axis_x * -(m_WNear / 2.0f) );

	GetPlaneEquation( m_Normals[PNEAR], m_Distances[PNEAR], m_points[ntl], m_points[ntr], m_points[nbr] );
	GetPlaneEquation( m_Normals[PFAR], m_Distances[PFAR], m_points[fbl], m_points[fbr], m_points[ftr] );
	GetPlaneEquation( m_Normals[PLEFT], m_Distances[PLEFT], m_points[ftl], m_points[ntl], m_points[nbl] );
	GetPlaneEquation( m_Normals[PRIGHT], m_Distances[PRIGHT], m_points[ntr], m_points[ftr], m_points[fbr] );
	GetPlaneEquation( m_Normals[PTOP], m_Distances[PTOP], m_points[ntl], m_points[ftl], m_points[ftr] );
	GetPlaneEquation( m_Normals[PBOTTOM], m_Distances[PBOTTOM], m_points[fbl], m_points[nbl], m_points[nbr] );
}

void EDFrustum::Render(void)
{
	glColor3f( 1.0f, 0.0f, 0.0f );
	glBegin( GL_LINES );
		glVertex3fv( m_points[ftl].v );
		glVertex3fv( m_points[ntl].v );
	
		glVertex3fv( m_points[ftr].v );
		glVertex3fv( m_points[ntr].v );

		glVertex3fv( m_points[fbl].v );
		glVertex3fv( m_points[nbl].v );

		glVertex3fv( m_points[fbr].v );
		glVertex3fv( m_points[nbr].v );

		glVertex3fv( m_points[ftl].v );
		glVertex3fv( m_points[ftr].v );

		glVertex3fv( m_points[fbl].v );
		glVertex3fv( m_points[fbr].v );

		glVertex3fv( m_points[ftl].v );
		glVertex3fv( m_points[fbl].v );

		glVertex3fv( m_points[ftr].v );
		glVertex3fv( m_points[fbr].v );

		glVertex3fv( m_points[ntl].v );
		glVertex3fv( m_points[ntr].v );

		glVertex3fv( m_points[nbl].v );
		glVertex3fv( m_points[nbr].v );

		glVertex3fv( m_points[ntl].v );
		glVertex3fv( m_points[nbl].v );

		glVertex3fv( m_points[ntr].v );
		glVertex3fv( m_points[nbr].v );
	glEnd();

	vec3f centroid;
	vec3f temp;


	glBegin( GL_LINES );
		centroid = ( m_points[ftl] + m_points[fbl] + m_points[nbl] + m_points[ntl] ) * 0.25f;
		temp = centroid + m_Normals[PLEFT];

		glColor3f( 0.0f, 0.0f, 1.0f );
		glVertex3fv( centroid.v );
		glColor3f( 1.0f, 1.0f, 1.0f );
		glVertex3fv( temp.v );

		centroid = ( m_points[ftr] + m_points[fbr] + m_points[nbr] + m_points[ntr] ) * 0.25f;
		temp = centroid + m_Normals[PRIGHT];

		glColor3f( 0.0f, 0.0f, 1.0f );
		glVertex3fv( centroid.v );
		glColor3f( 1.0f, 1.0f, 1.0f );
		glVertex3fv( temp.v );

		centroid = ( m_points[ftr] + m_points[ftl] + m_points[ntr] + m_points[ntl] ) * 0.25f;
		temp = centroid + m_Normals[PTOP];

		glColor3f( 0.0f, 0.0f, 1.0f );
		glVertex3fv( centroid.v );
		glColor3f( 1.0f, 1.0f, 1.0f );
		glVertex3fv( temp.v );

		centroid = ( m_points[fbr] + m_points[fbl] + m_points[nbr] + m_points[nbl] ) * 0.25f;
		temp = centroid + m_Normals[PBOTTOM];

		glColor3f( 0.0f, 0.0f, 1.0f );
		glVertex3fv( centroid.v );
		glColor3f( 1.0f, 1.0f, 1.0f );
		glVertex3fv( temp.v );

		centroid = ( m_points[nbr] + m_points[nbl] + m_points[ntr] + m_points[ntl] ) * 0.25f;
		temp = centroid + m_Normals[PNEAR];

		glColor3f( 0.0f, 0.0f, 1.0f );
		glVertex3fv( centroid.v );
		glColor3f( 1.0f, 1.0f, 1.0f );
		glVertex3fv( temp.v );

		centroid = ( m_points[fbr] + m_points[fbl] + m_points[ftr] + m_points[ftl] ) * 0.25f;
		temp = centroid + m_Normals[PFAR];

		glColor3f( 0.0f, 0.0f, 1.0f );
		glVertex3fv( centroid.v );
		glColor3f( 1.0f, 1.0f, 1.0f );
		glVertex3fv( temp.v );
	glEnd();

	glColor4f( 0.5f, 0.0f, 0.0f, 0.25f );
	glDisable( GL_LIGHTING );
	glCullFace( GL_FRONT );
	glBegin( GL_QUADS );
		
		glVertex3fv( m_points[ftl].v );
		glVertex3fv( m_points[ftr].v );
		glVertex3fv( m_points[fbr].v );
		glVertex3fv( m_points[fbl].v );

		glVertex3fv( m_points[ftl].v );
		glVertex3fv( m_points[fbl].v );
		glVertex3fv( m_points[nbl].v );
		glVertex3fv( m_points[ntl].v );

		glVertex3fv( m_points[ftr].v );
		glVertex3fv( m_points[ntr].v );
		glVertex3fv( m_points[nbr].v );
		glVertex3fv( m_points[fbr].v );

		glVertex3fv( m_points[ftl].v );
		glVertex3fv( m_points[ntl].v );
		glVertex3fv( m_points[ntr].v );
		glVertex3fv( m_points[ftr].v );

		glVertex3fv( m_points[fbl].v );
		glVertex3fv( m_points[fbr].v );
		glVertex3fv( m_points[nbr].v );
		glVertex3fv( m_points[nbl].v );

	glEnd();
	glCullFace( GL_BACK );
	glBegin( GL_QUADS );
		
		glVertex3fv( m_points[ftl].v );
		glVertex3fv( m_points[ftr].v );
		glVertex3fv( m_points[fbr].v );
		glVertex3fv( m_points[fbl].v );

		glVertex3fv( m_points[ftl].v );
		glVertex3fv( m_points[fbl].v );
		glVertex3fv( m_points[nbl].v );
		glVertex3fv( m_points[ntl].v );

		glVertex3fv( m_points[ftr].v );
		glVertex3fv( m_points[ntr].v );
		glVertex3fv( m_points[nbr].v );
		glVertex3fv( m_points[fbr].v );

		glVertex3fv( m_points[ftl].v );
		glVertex3fv( m_points[ntl].v );
		glVertex3fv( m_points[ntr].v );
		glVertex3fv( m_points[ftr].v );

		glVertex3fv( m_points[fbl].v );
		glVertex3fv( m_points[fbr].v );
		glVertex3fv( m_points[nbr].v );
		glVertex3fv( m_points[nbl].v );

	glEnd();
	glEnable( GL_LIGHTING );
}

void GetPlaneEquation( vec3f &Normal, float &Distance, const vec3f &Point1, const vec3f &Point2, const vec3f &Point3 )
{
	vec3f vec1(0, 0, 0);
	vec3f vec2(0, 0, 0);

	vec1 = Point1 - Point2;
	vec2 = Point2 - Point3;

	cross_product(Normal, vec1, vec2);
	Normal.normalize();

	Distance = dot_product(Point1, Normal);
}

bool EDFrustum::TestAABB( EDAABB& box )
{
	// The opposing corner pairs of the AABB calculated from it's min and max points
	vec3f corners[4][2];
	vec3f vectors[4];

	corners[0][0] = box.GetMin();
	corners[0][1] = box.GetMax();
	corners[1][0] = vec3f( box.GetMax().x, box.GetMin().y, box.GetMin().z );
	corners[1][1] = vec3f( box.GetMin().x, box.GetMax().y, box.GetMax().z );
	corners[2][0] = vec3f( box.GetMax().x, box.GetMin().y, box.GetMax().z );
	corners[2][1] = vec3f( box.GetMin().x, box.GetMax().y, box.GetMin().z );
	corners[3][0] = vec3f( box.GetMin().x, box.GetMin().y, box.GetMax().z );
	corners[3][1] = vec3f( box.GetMax().x, box.GetMax().y, box.GetMin().z );

	for (unsigned int i = 0; i < 4; ++i)
	{
		vectors[i] = corners[i][0] - corners[i][1];
	}

	float highest;
	int mostAligned;

	for (unsigned int q = 0; q < 6; ++q)
	{
		highest = 0.f;
		mostAligned = 0;


		for (unsigned int i = 0; i < 4; ++i)
		{
			float result = fabs(dot_product(vectors[i], m_Normals[q]));

			if (result > highest){
				highest = result;
				mostAligned = i;
			}
		}

		if ((dot_product(corners[mostAligned][0], m_Normals[q]) - m_Distances[q]) < 0.0f && (dot_product(corners[mostAligned][1], m_Normals[q]) - m_Distances[q]) < 0.0f)
		{ // if both are behind the plane
			return false;
		}

		//otherwise, continue testing.

	}

	return true;
}

bool EDFrustum::TestSphere( EDSphere& sphere )
{
	for (unsigned int q = 0; q < 6; ++q)
	{
		if (dot_product(sphere.GetCenter(), m_Normals[q]) + sphere.GetRadius() - m_Distances[q] < 0.0f)
			return false;
	}
	return true;

}
